package endcourseproject;

import java.util.ArrayList;
import java.util.Random;

import lejos.nxt.LCD;



public class Flock extends Behavior {

	private RobotManager _manager;
	private DirectionSensor _sensor;
	
	private int _direction = 0;
	private float _magnitude = 0;
	// TODO: consider making random access function available in Common or Util (maybe combine the two)
	
	public static final int UNKNOWN = 0;
	public static final int IN_FLOCK = 1;
	public static final int LOOKING_FOR_FLOCK = 2;
	public static final int FAR_FROM_FLOCK = 3;
	
	//public static final int FLOCK_SAFEZONE_THRESHOLD = 0x70;
	public static final int FLOCK_SAFEZONE_THRESHOLD = 0x80;
	//public static final int FLOCK_PERIPHEARALZONE_THRESHOLD = 0x40;
	public static final int FLOCK_PERIPHEARALZONE_THRESHOLD = 0x60;
	//public static final int FLOCK_UNSAFEZONE_THRESHOLD = 0x20;
	public static final int FLOCK_UNSAFEZONE_THRESHOLD = 0x40;
	public static final int FLOCK_MINIMUM_CHANGE = 10;
	
	public static int[] signalSum;
	
	
	public Flock(RobotManager manager, DirectionSensor sensor)
	{
		_manager = manager;
		_sensor = sensor;
	}
	
	private int _status = 0;
	private int _groupId = 0;
	private int _groupMemberCount = 1; // myself
	
	public int getGroupId()
	{
		return _groupId;
	}
	
	public int getStatus()
	{
		return _status;
	}
	
	public int getGroupMemberCount()
	{
		return _groupMemberCount;
	}
	
	private synchronized void set(int direction, float magnitude)
	{
		_direction = direction;
		_magnitude = magnitude;
	}
	
	@Override
	public synchronized int getDirection() {
		return _direction;
	}

	@Override
	public synchronized float getMagnitude() {
		return _magnitude;
	}
	
		
	public void run()
	{
		int count = -1;
		RobotInfo info = null;
		ArrayList<Integer> old_signals = new ArrayList<Integer>();
		int good_direction = 0;
		int current_direction = 0;
		int correction = 1;
		int correctionAngle = 0;
		 		
		signalSum = new int[3]; //TODO: set 3 to const
		
		while(true)
		{
			count = _manager.getNumFlockRobotInfo();
			
			
			
			for( int i = 0; i < count ;i++)
			{
				if( old_signals.size() <= i )
				{
					old_signals.add(-1);
				}
			}
			// before we update move old values
			
			
			for(int i = signalSum.length-1; i>0; i--) {
				  signalSum[i] = signalSum[i-1];
			}
			
			signalSum[0] = 0;
			
			int safeZone_cnt = 0;
			int periphiralZone_cnt = 0;
			int unSafeZone_cnt = 0;
			int further_cnt=0;
			int closer_cnt = 0;
			int same_cnt = 0;
						
			for(int i = 0; i < count; i++)
			{
				info = _manager.getFlockRobotInfo(i);
			
				// TODO: Check if robot is in my group/flock
				int old_signal = old_signals.get(i);
				int new_signal = info.getSignalStrength();
                
				// evaluate what zone we are in (hvor close we are to the other robots)
				if(new_signal>= FLOCK_SAFEZONE_THRESHOLD)
				{
				  safeZone_cnt++;	
				}else if(new_signal>= FLOCK_PERIPHEARALZONE_THRESHOLD)
				{
				  periphiralZone_cnt++;	
				}
				else //(new_signal<= FLOCK_UNSAFEZONE_THRESHOLD)
				{
				  unSafeZone_cnt++;	
				}
				
				signalSum[0] += new_signal;				
				old_signals.set(i, new_signal);
			}
			
			if (signalSum[0] > signalSum[1] && signalSum[0] > signalSum[2]) {
				correctionAngle = 60;
			}
			
			else {
				correctionAngle = 120;
			}
			
			//last_correction
			
			if (signalSum[1] > signalSum[2]) {
				correction *= -1;
			}
			
			correctionAngle *= correction;
			current_direction = _sensor.getDirection();
			
			// status set to notify other robots
			if((safeZone_cnt > periphiralZone_cnt && safeZone_cnt > unSafeZone_cnt )|| (safeZone_cnt == count))
			{
				_status = IN_FLOCK;
			}else if((periphiralZone_cnt  >= unSafeZone_cnt) || (periphiralZone_cnt == count))
			{
				_status = LOOKING_FOR_FLOCK;
			}else //if(unSafeZone_cnt> periphiralZone_cnt && unSafeZone_cnt > safeZone_cnt)
			{
				_status = FAR_FROM_FLOCK;
			}	
			
			if (_status == FAR_FROM_FLOCK) {
				set((current_direction+correctionAngle)% 360, 10);
				_manager.getRobotStatus().setFlockMembers(unSafeZone_cnt);
			}
			
			else if (_status == LOOKING_FOR_FLOCK) {
				set((current_direction+correctionAngle)% 360, 5);
				_manager.getRobotStatus().setFlockMembers(periphiralZone_cnt);
			}
			
			else {
				set((current_direction+correctionAngle)% 360, 0);
				_manager.getRobotStatus().setFlockMembers(safeZone_cnt);
			}
		
			//LCD.drawString("f="+further_cnt+",c="+closer_cnt+",s="+same_cnt, 0, 4);
			LCD.drawString("cangle:"+correctionAngle+" corr:"+correction, 0, 4);
			LCD.drawString("s="+safeZone_cnt+",p="+periphiralZone_cnt+",u="+unSafeZone_cnt, 0, 6);

			_manager.getRobotStatus().setFlockStatus(_status);
			_manager.getRobotStatus().setFlockID(count);
			Common.sleep(5000);
			info = null;
			
		}
	}
	
}
